% rotate points by Quaternion  and t
% q = q0 + q1*i + q2*j + q3*k;
% t = [x y z]
function rpoints = rotate3(points, q, t)
  q0 = q(1);
  q1 = q(2);
  q2 = q(3);
  q3 = q(4);
  R = [1-2*q2^2-2*q3^2, 2*q1*q2+2*q0*q3,2*q1*q3-2*q0*q2;
      2*q1*q2-2*q0*q3, 1-2*q1^2-2*q3^2, 2*q2*q3+2*q0*q1;
      2*q1*q3+2*q0*q2, 2*q2*q3-2*q0*q1, 1-2*q1^2-2*q2^2];
  for i=1:size(points)(1)
    rpoints(i,:) = (R*points(i,:)' + t')';
  endfor
endfunction